//
// Created by fazhehy on 2024/3/25.
//

#include "gnss.h"

uint16_t gnss_target_point_len = 0;
gnss_point_t gnss_target_point[50];

void bubble_sort(double * arr, uint8_t n)
{
    double temp;
    for (int i = 0; i < n-1; ++i) {
        for (int j = 0; j < n - 1 - i; ++j) {
            if (arr[j] > arr[j+1]){
                temp = arr[j + 1];
                arr[j + 1] = arr[j];
                arr[j] = temp;
            }
        }
    }
}

//void test()
//{
//    double arr[8] = {0.8, 0.008, 43, 66 ,58, 43,7 ,89};
//    bubble_sort(arr, 8);
//    for (int i = 0; i < 8; ++i) {
//        printf("%f ", arr[i]);
//    }
//    printf("\n");
//}

double latitude_temp[8] = {0};
double longitude_temp[8]= {0};
uint8_t gnss_record_point(uint8_t num)
{
//    while (gnss_flag == 0){
//        nrf24l01_printf("error\n");
//    }
//    gnss_data_parse();
//    if (gnss.state){
//        gnss_target_point[gnss_target_point_len].latitude = gnss.latitude;
//        gnss_target_point[gnss_target_point_len].longitude = gnss.longitude;
//        gnss_target_point_len ++;
//    }
//    gnss_flag = 0;

    for (int i = 0; i < 8; ++i) {
        while (gnss_flag == 0){
            __NOP();
            __NOP();
        }
        gnss_data_parse();
        if (gnss.state){
            latitude_temp[i] = gnss.latitude;
            longitude_temp[i] = gnss.longitude;
        }
        gnss_flag = 0;
    }

    bubble_sort(latitude_temp, 8);
    bubble_sort(longitude_temp, 8);

    gnss_target_point[num].latitude = 0;
    gnss_target_point[num].longitude = 0;

    for (int i = 1; i < 7; ++i) {
        gnss_target_point[num].latitude += latitude_temp[i];
        gnss_target_point[num].longitude += longitude_temp[i];
    }

    gnss_target_point[num].latitude /= 6;
    gnss_target_point[num].longitude /= 6;
//    num ++;

    return 0;
}

void calculate_center_point()
{
    uint8_t len = 0;
    double lat = 0;
    double lon = 0;
    for (int i = 0; i < 25; ++i) {
        if (car[i] == CAR_NUM){
            lat += gnss_target_point[i].latitude;
            lon += gnss_target_point[i].longitude;
            len ++;
        }
    }
    if (len != 0){
        gnss_target_point[0].latitude = lat/len;
        gnss_target_point[0].longitude = lon/len;
    }
}
